#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np

class MMWPointBase:
    def __init__(self):
        '''
        x(m),y(m),z(m),ref
        '''
        self.x = 0
        self.y = 0
        self.z = 0
        self.ref = 0

    def __str__(self):
        return f'{self.x}, {self.y}, {self.z}, {self.ref}'

class MMWPoinsCloud:
    def __init__(self):
        self.points = []

    def add_point(self, point):
        self.points.append(point)

    def get_point(self, index):
        return self.points[index]

    def get_points(self):
        return self.points

    def get_points_num(self):
        return len(self.points)

    def clear(self):
        self.points.clear()

    def __len__(self):
        return len(self.points)

    def __getitem__(self, index):
        return self.points[index]

    def __setitem__(self, index, value):
        self.points[index] = value

    def __delitem__(self, index):
        del self.points[index]

    def __iter__(self):
        return iter(self.points)

    def __reversed__(self):
        return reversed(self.points)

    def __add__(self, other):
        new = MMWPoinsCloud()
        new.points = self.points + other.points
        return new

    def __iadd__(self, other):
        self.points += other.points
        return self
    
    def save_csv(self, filename):
        with open(filename, 'w') as f:
            for point in self.__points:
                f.write(f'{point.x()}, {point.y()}, {point.z()}, {point.ref}\n')

    def save_pcd(self, filename):
        with open(filename, 'w') as f:
            f.write("# .PCD v0.7 - Point Cloud Data file format\n")
            f.write("VERSION 0.7\n")
            f.write("FIELDS x y z reflectivity\n")
            f.write("SIZE 4 4 4 4\n")
            f.write("TYPE F F F F\n")
            f.write("COUNT 1 1 1 1\n")
            f.write(f"WIDTH {len(self.points)}\n")
            f.write("HEIGHT 1\n")
            f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
            f.write(f"POINTS {len(self.points)}\n")
            f.write("DATA ascii\n")
            for i in range(len(self.points)):
                point = self.points[i]
                f.write(f'{point.x} {point.y} {point.z} {point.ref}\n')

class Frame:
    def __init__(self, timestamp: int, points: MMWPoinsCloud):
        self.timestamp = timestamp
        self.points = points

    def __str__(self):
        return f'timestamp: {self.timestamp}, points: {len(self.points)}'
    
    def __len__(self):
        return len(self.points)
    
class Frames:
    def __init__(self):
        self.frames = []

    def add_frame(self, frame):
        self.frames.append(frame)

    def get_frame(self, index):
        return self.frames[index]
    
    def get_frames(self):
        return self.frames
    
    def get_frames_num(self):
        return len(self.frames)
    
    def clear(self):
        self.frames.clear()

    def __len__(self):
        return len(self.frames)
    
    def __getitem__(self, index):
        return self.frames[index]
    
    def __setitem__(self, index, value):
        self.frames[index] = value

    def __delitem__(self, index):
        del self.frames[index]

    def __iter__(self):
        return iter(self.frames)
    
    def __reversed__(self):
        return reversed(self.frames)
    
    def __add__(self, other):
        new = Frames()
        new.frames = self.frames + other.frames
        return new
    
    def __iadd__(self, other):
        self.frames += other.frames
        return self
